#include "Inosv660nDrive.h"

InoSV660NDrive::InoSV660NDrive(unsigned int eep_man, unsigned int eep_id, const std::string& slave_name, int slave_addr,
                               ECatDrv::slaveType slave_type)
    :ECatDrv::ServoDrive(eep_man, eep_id, slave_name, slave_addr, slave_type)
{}

int InoSV660NDrive::configurePDO()
{
    int retval = 0;
    int indexSize;
    uint8 zero = 0;

    retval += ec_SDOwrite(slave_addr, 0x1c12, 0, FALSE, 2, &zero, EC_TIMEOUTSAFE);
    retval += ec_SDOwrite(slave_addr, 0x1c13, 0, FALSE, 2, &zero, EC_TIMEOUTSAFE);
    retval += ec_SDOwrite(slave_addr, 0x1A00, 0, FALSE, 2, &zero, EC_TIMEOUTSAFE);

    // 输入PDO映射（从站 -> 主站）
    uint32_t txPdoGroup[] = {
        0x60410010,  // Statusword
        0x60640020,  // Position actual value
        0x60B90010,  // Touch probe status
        0x60BA0020,  // Touch probe pos1 pos value
        0x60BC0020,  // Touch probe pos2 pos value
        0x603F0010,  // Error code
        0x60FD0020   // Digital inputs
    };

    indexSize = sizeof(txPdoGroup)/sizeof(uint32_t);
    for(int i = 0; i < indexSize; i++)
    {
        retval += ec_SDOwrite(slave_addr, 0x1A00, i + 1, FALSE, 4, &txPdoGroup[i], EC_TIMEOUTSAFE);
    }
    retval += ec_SDOwrite(slave_addr, 0x1A00, 0x00, FALSE, 2, &indexSize, EC_TIMEOUTSAFE);

    retval += ec_SDOwrite(slave_addr, 0x1600, 0, FALSE, 2, &zero, EC_TIMEOUTSAFE);
    // 输出PDO映射（主站 -> 从站）
    uint32_t rxPdoGroup[] = {
        0x60400010,  // Controlword
        0x607A0020,  // Target position
//        0x60FF0020,  // Target velocity
//        0x60710010,  // Target torque
//        0x60600008,  // Modes of operation
        0x60B80010,  // Touch probe function
//        0x607F0020   // Max profile velocity
    };

    indexSize = sizeof(rxPdoGroup)/sizeof(uint32_t);
    for(int i = 0; i < indexSize; i++)
    {
        retval += ec_SDOwrite(slave_addr, 0x1600, i + 1, FALSE, 4, &rxPdoGroup[i], EC_TIMEOUTSAFE);
    }
    retval += ec_SDOwrite(slave_addr, 0x1600, 0x00, FALSE, 2, &indexSize, EC_TIMEOUTSAFE);

    // 将PDO映射添加到同步管理器中
    ec_SDOwrite(slave_addr, 0x1C12, 0x00, FALSE, 1, &zero, EC_TIMEOUTSAFE);
    ec_SDOwrite(slave_addr, 0x1C13, 0x00, FALSE, 1, &zero, EC_TIMEOUTSAFE);

    uint16 rxpdo = 0x1600;
    uint16 txpdo = 0x1A00;  // 0x1B01

    uint8 rxnum = 1;
    uint8 txnum = 1;

    ec_SDOwrite(slave_addr, 0x1C12, 1, FALSE, 2, &rxpdo, EC_TIMEOUTSAFE);
    ec_SDOwrite(slave_addr, 0x1C12, 0x00, FALSE, 1, &rxnum, EC_TIMEOUTSAFE);

    ec_SDOwrite(slave_addr, 0x1C13, 1, FALSE, 2, &txpdo, EC_TIMEOUTSAFE);
    ec_SDOwrite(slave_addr, 0x1C13, 0x00, FALSE, 1, &txnum, EC_TIMEOUTSAFE);


//    uint8 eight = 8;
//    retval += ec_SDOwrite(slave, 0x6060, 0, FALSE, 1, &eight, EC_TIMEOUTSAFE);

//    uint32 map_1c12[] = {0x17020001};
//    uint32 map_1c13[] = {0x1A000001};

//    retval += ec_SDOwrite(slave, 0x1c12, 0x00, FALSE, sizeof (map_1c12), &map_1c12, EC_TIMEOUTSAFE);
//    retval += ec_SDOwrite(slave, 0x1c13, 0x00, FALSE, sizeof (map_1c13), &map_1c13, EC_TIMEOUTSAFE);

    while(EcatError) printf("%s\n", ec_elist2string());

    return 1;
}
